{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "###  基于采样的路径规划算法——RRT*\n",
    "\n",
    "博客参考：https://blog.csdn.net/weixin_42301220/article/details/125275337"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import os\n",
    "import sys\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "from celluloid import Camera  # 保存动图时用，pip install celluloid\n",
    "\n",
    "sys.path.append(\"../RRT\")\n",
    "try:\n",
    "    from rrt_planning import RRT\n",
    "except ImportError:\n",
    "    raise\n",
    "from rrt_star_planning import RRTStar\n",
    "show_animation = True\n",
    "\n",
    "%matplotlib qt5\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RRT*\n",
    "\n",
    "由于许多代码复用RRT的，所以直接继承了RRT类"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Start \n",
      "Iter: 0 , number of nodes: 1\n",
      "Iter: 1 , number of nodes: 2\n",
      "Iter: 2 , number of nodes: 3\n",
      "Iter: 3 , number of nodes: 4\n",
      "Iter: 4 , number of nodes: 5\n",
      "Iter: 5 , number of nodes: 5\n",
      "Iter: 6 , number of nodes: 5\n",
      "Iter: 7 , number of nodes: 5\n",
      "Iter: 8 , number of nodes: 5\n",
      "Iter: 9 , number of nodes: 5\n",
      "Iter: 10 , number of nodes: 5\n",
      "Iter: 11 , number of nodes: 6\n",
      "Iter: 12 , number of nodes: 6\n",
      "Iter: 13 , number of nodes: 7\n",
      "Iter: 14 , number of nodes: 8\n",
      "Iter: 15 , number of nodes: 8\n",
      "Iter: 16 , number of nodes: 9\n",
      "Iter: 17 , number of nodes: 9\n",
      "Iter: 18 , number of nodes: 10\n",
      "Iter: 19 , number of nodes: 10\n",
      "Iter: 20 , number of nodes: 10\n",
      "Iter: 21 , number of nodes: 10\n",
      "Iter: 22 , number of nodes: 10\n",
      "Iter: 23 , number of nodes: 10\n",
      "Iter: 24 , number of nodes: 10\n",
      "Iter: 25 , number of nodes: 10\n",
      "Iter: 26 , number of nodes: 10\n",
      "Iter: 27 , number of nodes: 10\n",
      "Iter: 28 , number of nodes: 10\n",
      "Iter: 29 , number of nodes: 10\n",
      "Iter: 30 , number of nodes: 10\n",
      "Iter: 31 , number of nodes: 10\n",
      "Iter: 32 , number of nodes: 10\n",
      "Iter: 33 , number of nodes: 10\n",
      "Iter: 34 , number of nodes: 10\n",
      "Iter: 35 , number of nodes: 10\n",
      "Iter: 36 , number of nodes: 10\n",
      "Iter: 37 , number of nodes: 10\n",
      "Iter: 38 , number of nodes: 10\n",
      "Iter: 39 , number of nodes: 10\n",
      "Iter: 40 , number of nodes: 10\n",
      "Iter: 41 , number of nodes: 10\n",
      "Iter: 42 , number of nodes: 11\n",
      "Iter: 43 , number of nodes: 11\n",
      "Iter: 44 , number of nodes: 12\n",
      "Iter: 45 , number of nodes: 13\n",
      "Iter: 46 , number of nodes: 13\n",
      "Iter: 47 , number of nodes: 14\n",
      "Iter: 48 , number of nodes: 15\n",
      "Iter: 49 , number of nodes: 15\n",
      "Iter: 50 , number of nodes: 15\n",
      "Iter: 51 , number of nodes: 15\n",
      "Iter: 52 , number of nodes: 15\n",
      "Iter: 53 , number of nodes: 15\n",
      "Iter: 54 , number of nodes: 15\n",
      "Iter: 55 , number of nodes: 15\n",
      "Iter: 56 , number of nodes: 15\n",
      "Iter: 57 , number of nodes: 15\n",
      "Iter: 58 , number of nodes: 15\n",
      "Iter: 59 , number of nodes: 15\n",
      "Iter: 60 , number of nodes: 15\n",
      "Iter: 61 , number of nodes: 15\n",
      "Iter: 62 , number of nodes: 15\n",
      "Iter: 63 , number of nodes: 16\n",
      "Iter: 64 , number of nodes: 16\n",
      "Iter: 65 , number of nodes: 16\n",
      "Iter: 66 , number of nodes: 17\n",
      "Iter: 67 , number of nodes: 17\n",
      "Iter: 68 , number of nodes: 18\n",
      "Iter: 69 , number of nodes: 18\n",
      "Iter: 70 , number of nodes: 18\n",
      "Iter: 71 , number of nodes: 18\n",
      "Iter: 72 , number of nodes: 19\n",
      "Iter: 73 , number of nodes: 19\n",
      "Iter: 74 , number of nodes: 19\n",
      "Iter: 75 , number of nodes: 19\n",
      "Iter: 76 , number of nodes: 20\n",
      "Iter: 77 , number of nodes: 20\n",
      "Iter: 78 , number of nodes: 20\n",
      "Iter: 79 , number of nodes: 21\n",
      "Iter: 80 , number of nodes: 21\n",
      "Iter: 81 , number of nodes: 22\n",
      "Iter: 82 , number of nodes: 22\n",
      "Iter: 83 , number of nodes: 22\n",
      "Iter: 84 , number of nodes: 22\n",
      "Iter: 85 , number of nodes: 22\n",
      "Iter: 86 , number of nodes: 22\n",
      "Iter: 87 , number of nodes: 22\n",
      "Iter: 88 , number of nodes: 22\n",
      "Iter: 89 , number of nodes: 22\n",
      "Iter: 90 , number of nodes: 22\n",
      "Iter: 91 , number of nodes: 22\n",
      "Iter: 92 , number of nodes: 22\n",
      "Iter: 93 , number of nodes: 23\n",
      "Iter: 94 , number of nodes: 23\n",
      "Iter: 95 , number of nodes: 24\n",
      "Iter: 96 , number of nodes: 24\n",
      "Iter: 97 , number of nodes: 24\n",
      "Iter: 98 , number of nodes: 24\n",
      "Iter: 99 , number of nodes: 24\n",
      "Iter: 100 , number of nodes: 24\n",
      "Iter: 101 , number of nodes: 24\n",
      "Iter: 102 , number of nodes: 25\n",
      "Iter: 103 , number of nodes: 25\n",
      "Iter: 104 , number of nodes: 25\n",
      "Iter: 105 , number of nodes: 25\n",
      "Iter: 106 , number of nodes: 25\n",
      "Iter: 107 , number of nodes: 25\n",
      "Iter: 108 , number of nodes: 25\n",
      "Iter: 109 , number of nodes: 25\n",
      "Iter: 110 , number of nodes: 25\n",
      "Iter: 111 , number of nodes: 25\n",
      "Iter: 112 , number of nodes: 25\n",
      "Iter: 113 , number of nodes: 25\n",
      "Iter: 114 , number of nodes: 25\n",
      "Iter: 115 , number of nodes: 25\n",
      "Iter: 116 , number of nodes: 26\n",
      "Iter: 117 , number of nodes: 26\n",
      "Iter: 118 , number of nodes: 27\n",
      "Iter: 119 , number of nodes: 28\n",
      "Iter: 120 , number of nodes: 28\n",
      "Iter: 121 , number of nodes: 28\n",
      "Iter: 122 , number of nodes: 28\n",
      "Iter: 123 , number of nodes: 29\n",
      "Iter: 124 , number of nodes: 29\n",
      "Iter: 125 , number of nodes: 29\n",
      "Iter: 126 , number of nodes: 30\n",
      "Iter: 127 , number of nodes: 30\n",
      "Iter: 128 , number of nodes: 30\n",
      "Iter: 129 , number of nodes: 30\n",
      "Iter: 130 , number of nodes: 30\n",
      "Iter: 131 , number of nodes: 31\n",
      "Iter: 132 , number of nodes: 31\n",
      "Iter: 133 , number of nodes: 31\n",
      "Iter: 134 , number of nodes: 32\n",
      "Iter: 135 , number of nodes: 33\n",
      "Iter: 136 , number of nodes: 33\n",
      "Iter: 137 , number of nodes: 34\n",
      "Iter: 138 , number of nodes: 34\n",
      "Iter: 139 , number of nodes: 34\n",
      "Iter: 140 , number of nodes: 34\n",
      "Iter: 141 , number of nodes: 35\n",
      "Iter: 142 , number of nodes: 35\n",
      "Iter: 143 , number of nodes: 36\n",
      "Iter: 144 , number of nodes: 37\n",
      "Iter: 145 , number of nodes: 37\n",
      "Iter: 146 , number of nodes: 37\n",
      "Iter: 147 , number of nodes: 37\n",
      "Iter: 148 , number of nodes: 37\n",
      "Iter: 149 , number of nodes: 37\n",
      "Iter: 150 , number of nodes: 37\n",
      "Iter: 151 , number of nodes: 38\n",
      "Iter: 152 , number of nodes: 38\n",
      "Iter: 153 , number of nodes: 38\n",
      "Iter: 154 , number of nodes: 38\n",
      "Iter: 155 , number of nodes: 39\n",
      "Iter: 156 , number of nodes: 39\n",
      "Iter: 157 , number of nodes: 40\n",
      "Iter: 158 , number of nodes: 40\n",
      "Iter: 159 , number of nodes: 41\n",
      "Iter: 160 , number of nodes: 42\n",
      "Iter: 161 , number of nodes: 43\n",
      "Iter: 162 , number of nodes: 43\n",
      "Iter: 163 , number of nodes: 44\n",
      "Iter: 164 , number of nodes: 44\n",
      "Iter: 165 , number of nodes: 44\n",
      "Iter: 166 , number of nodes: 45\n",
      "Iter: 167 , number of nodes: 46\n",
      "Iter: 168 , number of nodes: 46\n",
      "Iter: 169 , number of nodes: 47\n",
      "Iter: 170 , number of nodes: 47\n",
      "Iter: 171 , number of nodes: 47\n",
      "Iter: 172 , number of nodes: 48\n",
      "Iter: 173 , number of nodes: 48\n",
      "Iter: 174 , number of nodes: 48\n",
      "Iter: 175 , number of nodes: 48\n",
      "Iter: 176 , number of nodes: 48\n",
      "Iter: 177 , number of nodes: 48\n",
      "Iter: 178 , number of nodes: 48\n",
      "Iter: 179 , number of nodes: 48\n",
      "Iter: 180 , number of nodes: 48\n",
      "Iter: 181 , number of nodes: 48\n",
      "Iter: 182 , number of nodes: 48\n",
      "Iter: 183 , number of nodes: 48\n",
      "Iter: 184 , number of nodes: 48\n",
      "Iter: 185 , number of nodes: 48\n",
      "Iter: 186 , number of nodes: 49\n",
      "Iter: 187 , number of nodes: 49\n",
      "Iter: 188 , number of nodes: 49\n",
      "Iter: 189 , number of nodes: 50\n",
      "Iter: 190 , number of nodes: 51\n",
      "Iter: 191 , number of nodes: 51\n",
      "Iter: 192 , number of nodes: 52\n",
      "Iter: 193 , number of nodes: 53\n",
      "Iter: 194 , number of nodes: 53\n",
      "Iter: 195 , number of nodes: 53\n",
      "Iter: 196 , number of nodes: 53\n",
      "Iter: 197 , number of nodes: 54\n",
      "Iter: 198 , number of nodes: 54\n",
      "Iter: 199 , number of nodes: 55\n",
      "Iter: 200 , number of nodes: 55\n",
      "Iter: 201 , number of nodes: 56\n",
      "Iter: 202 , number of nodes: 57\n",
      "Iter: 203 , number of nodes: 57\n",
      "Iter: 204 , number of nodes: 58\n",
      "Iter: 205 , number of nodes: 58\n",
      "Iter: 206 , number of nodes: 58\n",
      "Iter: 207 , number of nodes: 58\n",
      "Iter: 208 , number of nodes: 59\n",
      "Iter: 209 , number of nodes: 59\n",
      "Iter: 210 , number of nodes: 60\n",
      "Iter: 211 , number of nodes: 60\n",
      "Iter: 212 , number of nodes: 60\n",
      "Iter: 213 , number of nodes: 60\n",
      "Iter: 214 , number of nodes: 60\n",
      "Iter: 215 , number of nodes: 60\n",
      "Iter: 216 , number of nodes: 60\n",
      "Iter: 217 , number of nodes: 60\n",
      "Iter: 218 , number of nodes: 61\n",
      "Iter: 219 , number of nodes: 62\n",
      "Iter: 220 , number of nodes: 62\n",
      "Iter: 221 , number of nodes: 62\n",
      "Iter: 222 , number of nodes: 63\n",
      "Iter: 223 , number of nodes: 64\n",
      "Iter: 224 , number of nodes: 65\n",
      "Iter: 225 , number of nodes: 66\n",
      "Iter: 226 , number of nodes: 67\n",
      "Iter: 227 , number of nodes: 68\n",
      "Iter: 228 , number of nodes: 68\n",
      "Iter: 229 , number of nodes: 69\n",
      "Iter: 230 , number of nodes: 70\n",
      "Iter: 231 , number of nodes: 70\n",
      "Iter: 232 , number of nodes: 70\n",
      "Iter: 233 , number of nodes: 71\n",
      "Iter: 234 , number of nodes: 71\n",
      "Iter: 235 , number of nodes: 71\n",
      "Iter: 236 , number of nodes: 72\n",
      "Iter: 237 , number of nodes: 73\n",
      "Iter: 238 , number of nodes: 74\n",
      "Iter: 239 , number of nodes: 74\n",
      "Iter: 240 , number of nodes: 74\n",
      "Iter: 241 , number of nodes: 74\n",
      "Iter: 242 , number of nodes: 75\n",
      "Iter: 243 , number of nodes: 76\n",
      "Iter: 244 , number of nodes: 76\n",
      "Iter: 245 , number of nodes: 77\n",
      "Iter: 246 , number of nodes: 77\n",
      "Iter: 247 , number of nodes: 78\n",
      "Iter: 248 , number of nodes: 79\n",
      "Iter: 249 , number of nodes: 79\n",
      "Iter: 250 , number of nodes: 80\n",
      "Iter: 251 , number of nodes: 80\n",
      "Iter: 252 , number of nodes: 81\n",
      "Iter: 253 , number of nodes: 82\n",
      "Iter: 254 , number of nodes: 83\n",
      "Iter: 255 , number of nodes: 84\n",
      "Iter: 256 , number of nodes: 84\n",
      "Iter: 257 , number of nodes: 85\n",
      "Iter: 258 , number of nodes: 85\n",
      "Iter: 259 , number of nodes: 85\n",
      "Iter: 260 , number of nodes: 85\n",
      "Iter: 261 , number of nodes: 86\n",
      "Iter: 262 , number of nodes: 86\n",
      "Iter: 263 , number of nodes: 87\n",
      "Iter: 264 , number of nodes: 87\n",
      "Iter: 265 , number of nodes: 88\n",
      "Iter: 266 , number of nodes: 89\n",
      "Iter: 267 , number of nodes: 90\n",
      "Iter: 268 , number of nodes: 91\n",
      "Iter: 269 , number of nodes: 92\n",
      "Iter: 270 , number of nodes: 92\n",
      "Iter: 271 , number of nodes: 92\n",
      "Iter: 272 , number of nodes: 93\n",
      "Iter: 273 , number of nodes: 93\n",
      "Iter: 274 , number of nodes: 94\n",
      "Iter: 275 , number of nodes: 95\n",
      "Iter: 276 , number of nodes: 96\n",
      "Iter: 277 , number of nodes: 96\n",
      "Iter: 278 , number of nodes: 97\n",
      "Iter: 279 , number of nodes: 98\n",
      "Iter: 280 , number of nodes: 99\n",
      "Iter: 281 , number of nodes: 100\n",
      "Iter: 282 , number of nodes: 101\n",
      "Iter: 283 , number of nodes: 102\n",
      "Iter: 284 , number of nodes: 102\n",
      "Iter: 285 , number of nodes: 103\n",
      "Iter: 286 , number of nodes: 104\n",
      "Iter: 287 , number of nodes: 104\n",
      "Iter: 288 , number of nodes: 105\n",
      "Iter: 289 , number of nodes: 105\n",
      "Iter: 290 , number of nodes: 105\n",
      "Iter: 291 , number of nodes: 106\n",
      "Iter: 292 , number of nodes: 106\n",
      "Iter: 293 , number of nodes: 107\n",
      "Iter: 294 , number of nodes: 108\n",
      "Iter: 295 , number of nodes: 109\n",
      "Iter: 296 , number of nodes: 110\n",
      "Iter: 297 , number of nodes: 111\n",
      "Iter: 298 , number of nodes: 112\n",
      "Iter: 299 , number of nodes: 113\n",
      "Iter: 300 , number of nodes: 114\n",
      "Iter: 301 , number of nodes: 114\n",
      "Iter: 302 , number of nodes: 115\n",
      "Iter: 303 , number of nodes: 116\n",
      "Iter: 304 , number of nodes: 116\n",
      "Iter: 305 , number of nodes: 116\n",
      "Iter: 306 , number of nodes: 116\n",
      "Iter: 307 , number of nodes: 116\n",
      "Iter: 308 , number of nodes: 117\n",
      "Iter: 309 , number of nodes: 118\n",
      "Iter: 310 , number of nodes: 118\n",
      "Iter: 311 , number of nodes: 119\n",
      "Iter: 312 , number of nodes: 119\n",
      "Iter: 313 , number of nodes: 119\n",
      "Iter: 314 , number of nodes: 120\n",
      "Iter: 315 , number of nodes: 120\n",
      "Iter: 316 , number of nodes: 120\n",
      "Iter: 317 , number of nodes: 121\n",
      "Iter: 318 , number of nodes: 122\n",
      "Iter: 319 , number of nodes: 122\n",
      "Iter: 320 , number of nodes: 122\n",
      "Iter: 321 , number of nodes: 122\n",
      "Iter: 322 , number of nodes: 123\n",
      "Iter: 323 , number of nodes: 123\n",
      "Iter: 324 , number of nodes: 124\n",
      "Iter: 325 , number of nodes: 125\n",
      "Iter: 326 , number of nodes: 125\n",
      "Iter: 327 , number of nodes: 126\n",
      "Iter: 328 , number of nodes: 126\n",
      "Iter: 329 , number of nodes: 126\n",
      "Iter: 330 , number of nodes: 127\n",
      "Iter: 331 , number of nodes: 128\n",
      "Iter: 332 , number of nodes: 128\n",
      "Iter: 333 , number of nodes: 128\n",
      "Iter: 334 , number of nodes: 128\n",
      "Iter: 335 , number of nodes: 129\n",
      "Iter: 336 , number of nodes: 130\n",
      "Iter: 337 , number of nodes: 130\n",
      "Iter: 338 , number of nodes: 130\n",
      "Iter: 339 , number of nodes: 131\n",
      "Iter: 340 , number of nodes: 131\n",
      "Iter: 341 , number of nodes: 131\n",
      "Iter: 342 , number of nodes: 132\n",
      "Iter: 343 , number of nodes: 132\n",
      "Iter: 344 , number of nodes: 133\n",
      "Iter: 345 , number of nodes: 133\n",
      "Iter: 346 , number of nodes: 134\n",
      "Iter: 347 , number of nodes: 134\n",
      "Iter: 348 , number of nodes: 134\n",
      "Iter: 349 , number of nodes: 135\n",
      "Iter: 350 , number of nodes: 135\n",
      "Iter: 351 , number of nodes: 136\n",
      "Iter: 352 , number of nodes: 136\n",
      "Iter: 353 , number of nodes: 137\n",
      "Iter: 354 , number of nodes: 138\n",
      "Iter: 355 , number of nodes: 139\n",
      "Iter: 356 , number of nodes: 139\n",
      "Iter: 357 , number of nodes: 140\n",
      "Iter: 358 , number of nodes: 141\n",
      "Iter: 359 , number of nodes: 141\n",
      "Iter: 360 , number of nodes: 141\n",
      "Iter: 361 , number of nodes: 142\n",
      "Iter: 362 , number of nodes: 143\n",
      "Iter: 363 , number of nodes: 144\n",
      "Iter: 364 , number of nodes: 145\n",
      "Iter: 365 , number of nodes: 146\n",
      "Iter: 366 , number of nodes: 146\n",
      "Iter: 367 , number of nodes: 146\n",
      "Iter: 368 , number of nodes: 147\n",
      "Iter: 369 , number of nodes: 148\n",
      "Iter: 370 , number of nodes: 148\n",
      "Iter: 371 , number of nodes: 149\n",
      "Iter: 372 , number of nodes: 150\n",
      "Iter: 373 , number of nodes: 150\n",
      "Iter: 374 , number of nodes: 151\n",
      "Iter: 375 , number of nodes: 152\n",
      "Iter: 376 , number of nodes: 152\n",
      "Iter: 377 , number of nodes: 152\n",
      "Iter: 378 , number of nodes: 153\n",
      "Iter: 379 , number of nodes: 154\n",
      "Iter: 380 , number of nodes: 155\n",
      "Iter: 381 , number of nodes: 155\n",
      "Iter: 382 , number of nodes: 156\n",
      "Iter: 383 , number of nodes: 157\n",
      "Iter: 384 , number of nodes: 157\n",
      "Iter: 385 , number of nodes: 158\n",
      "Iter: 386 , number of nodes: 159\n",
      "Iter: 387 , number of nodes: 160\n",
      "Iter: 388 , number of nodes: 161\n",
      "Iter: 389 , number of nodes: 161\n",
      "Iter: 390 , number of nodes: 162\n",
      "Iter: 391 , number of nodes: 162\n",
      "Iter: 392 , number of nodes: 163\n",
      "Iter: 393 , number of nodes: 163\n",
      "Iter: 394 , number of nodes: 163\n",
      "Iter: 395 , number of nodes: 163\n",
      "Iter: 396 , number of nodes: 164\n",
      "Iter: 397 , number of nodes: 165\n",
      "Iter: 398 , number of nodes: 165\n",
      "Iter: 399 , number of nodes: 165\n",
      "Iter: 400 , number of nodes: 166\n",
      "Iter: 401 , number of nodes: 167\n",
      "Iter: 402 , number of nodes: 168\n",
      "Iter: 403 , number of nodes: 168\n",
      "Iter: 404 , number of nodes: 169\n",
      "Iter: 405 , number of nodes: 169\n",
      "Iter: 406 , number of nodes: 170\n",
      "Iter: 407 , number of nodes: 170\n",
      "Iter: 408 , number of nodes: 170\n",
      "Iter: 409 , number of nodes: 171\n",
      "Iter: 410 , number of nodes: 172\n",
      "Iter: 411 , number of nodes: 173\n",
      "Iter: 412 , number of nodes: 174\n",
      "Iter: 413 , number of nodes: 175\n",
      "Iter: 414 , number of nodes: 175\n",
      "Iter: 415 , number of nodes: 175\n",
      "Iter: 416 , number of nodes: 175\n",
      "Iter: 417 , number of nodes: 176\n",
      "Iter: 418 , number of nodes: 177\n",
      "Iter: 419 , number of nodes: 178\n",
      "Iter: 420 , number of nodes: 179\n",
      "Iter: 421 , number of nodes: 179\n",
      "Iter: 422 , number of nodes: 180\n",
      "Iter: 423 , number of nodes: 180\n",
      "Iter: 424 , number of nodes: 180\n",
      "Iter: 425 , number of nodes: 181\n",
      "Iter: 426 , number of nodes: 181\n",
      "Iter: 427 , number of nodes: 181\n",
      "Iter: 428 , number of nodes: 182\n",
      "Iter: 429 , number of nodes: 182\n",
      "Iter: 430 , number of nodes: 182\n",
      "Iter: 431 , number of nodes: 182\n",
      "Iter: 432 , number of nodes: 182\n",
      "Iter: 433 , number of nodes: 183\n",
      "Iter: 434 , number of nodes: 183\n",
      "Iter: 435 , number of nodes: 184\n"
     ]
    }
   ],
   "source": [
    "def main():\n",
    "    print(\"Start \")\n",
    "    fig = plt.figure(1)\n",
    "\n",
    "    camera = Camera(fig)  # 保存动图时使用\n",
    "    # camera = None # 不保存动图时，camara为None\n",
    "    show_animation = True\n",
    "    # ====Search Path with RRT====\n",
    "    obstacle_list = [\n",
    "        (5, 5, 1),\n",
    "        (3, 6, 2),\n",
    "        (3, 8, 2),\n",
    "        (3, 10, 2),\n",
    "        (7, 5, 2),\n",
    "        (9, 5, 2),\n",
    "        (8, 10, 1),\n",
    "        (6, 12, 1),\n",
    "    ]  # [x,y,size(radius)]\n",
    "\n",
    "    # Set Initial parameters\n",
    "    rrt_star = RRTStar(\n",
    "        start=[0, 0],\n",
    "        goal=[6, 10],\n",
    "        rand_area=[-2, 15],\n",
    "        obstacle_list=obstacle_list,\n",
    "        expand_dis=3,\n",
    "        robot_radius=0.8)\n",
    "    path = rrt_star.planning(animation=show_animation, camera=None)\n",
    "\n",
    "    if path is None:\n",
    "        print(\"Cannot find path\")\n",
    "    else:\n",
    "        print(\"found path!!\")\n",
    "\n",
    "        # Draw final path\n",
    "        if show_animation:\n",
    "            plt.cla()\n",
    "            rrt_star.draw_graph(camera=None)\n",
    "            plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--')\n",
    "            plt.grid(True)\n",
    "            # if camera != None:\n",
    "            #     camera.snap()\n",
    "            #     animation = camera.animate()\n",
    "                # animation.save('trajectory.gif')\n",
    "    plt.show()\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "main()\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.12 ('tftorch')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "49392358061de69c353098f25d40d84850d9a894aad989afedb5c9a02f15f84b"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
